# coding=utf-8
import cv2
import RPi.GPIO as GPIO
import time
import urllib
import numpy as np

#def set_cloud_platform_degree(ao):
    #r = 10 / 180 * ao + 2
    #p.ChangeDutyCycle(r)
            #通过用户输入的角度来改变舵机的角度
    #time.sleep(1)
P_SERVO = 7 # GPIO端口号，根据实际修改
fPWM = 50  # Hz (软件PWM方式，频率不能设置过高)
a = 10
b = 2

    # Vcc_Pin=37
In_Pin=4
#操控线（黄线）
def setup():
    global pwm
    GPIO.setmode(GPIO.BOARD)
    GPIO.setup(P_SERVO, GPIO.OUT)
    pwm = GPIO.PWM(P_SERVO, fPWM)
    pwm.start(0)
# GPIO.setup(Vcc_Pin,GPIO.OUT,initial=GPIO.HIGH)
def setDirection(direction):
    duty = int(((a * (180-direction)) / 180)  + b)
    pwm.ChangeDutyCycle(duty)
    #print ("direction =", direction, "-> duty =", duty)
    time.sleep(0.1)
    
last_btm_degree = 100 # 最近一次底部舵机的角度值记录
last_top_degree = 100 # 最近一次顶部舵机的角度值记录

btm_kp = 20 # 底部舵机的Kp系数
top_kp = 5 # 顶部舵机的Kp系数

offset_dead_block = 0.1 # 设置偏移量的死区

# 舵机角度初始化
setup()
setDirection(90) 

# 载入人脸检测的Cascade模型
FaceCascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')

# 创建一个窗口 名字叫做Face
#cv2.namedWindow('FaceDetect',0)
#cv2.resizeWindow("FaceDetect", 480, 360);
cap = cv2.VideoCapture(-1)  #Turn on the camera
cap.set(3,640)
cap.set(4,480)

#url='http://192.168.43.31/cam-hi.jpg'

def getimg( ):   
    ret,frame = cap.read()
    img21 = cv2.resize(frame,(160,120))       
    return img21
    # all the opencv processing is done here
    



def btm_servo_control(offset_x):
    # '''
    # 底部舵机的比例控制
    # 这里舵机使用开环控制
    # '''
    global offset_dead_block # 偏移量死区大小
    global btm_kp # 控制舵机旋转的比例系数
    global last_btm_degree # 上一次底部舵机的角度
    
    # 设置最小阈值
    if abs(offset_x) < offset_dead_block:
       offset_x = 0

    # offset范围在-50到50左右
    delta_degree = offset_x * btm_kp
    # 计算得到新的底部舵机角度
    next_btm_degree = last_btm_degree + delta_degree
    print("jiao du"+str(next_btm_degree))
    # 添加边界检测
    if next_btm_degree < 0:
        next_btm_degree = 0
    elif next_btm_degree > 180:
        next_btm_degree = 180
    
    return int(next_btm_degree)

def top_servo_control(offset_y):
    # '''
    # 顶部舵机的比例控制
    # 这里舵机使用开环控制
    # '''
    global offset_dead_block
    global top_kp # 控制舵机旋转的比例系数
    global last_top_degree # 上一次顶部舵机的角度

    # 如果偏移量小于阈值就不相应
    if abs(offset_y) < offset_dead_block:
        offset_y = 0

    # offset_y *= -1
    # offset范围在-50到50左右
    delta_degree = offset_y * top_kp
    # 新的顶部舵机角度
    next_top_degree = last_top_degree + delta_degree
    # 添加边界检测
    if next_top_degree < 0:
        next_top_degree = 0
    elif next_top_degree > 180:
        next_top_degree = 180
    
    return int(next_top_degree)


def face_filter(faces):
    # '''
    # 对人脸进行一个过滤
    # '''
    if len(faces) == 0:
        return None
    
    # 目前找的是画面中面积最大的人脸
    max_face =  max(faces, key=lambda face: face[2]*face[3])
    (x, y, w, h) = max_face
    if w < 10 or h < 10:
        return None
    return max_face

def calculate_offset(img_width, img_height, face):
    # '''
    # 计算人脸在画面中的偏移量
    # 偏移量的取值范围： [-1, 1]
    # '''
    (x, y, w, h) = face
    face_x = float(x + w/2.0)
    face_y = float(y + h/2.0)
    # 人脸在画面中心X轴上的偏移量
    offset_x = float(face_x / img_width - 0.5) * 2
    # 人脸在画面中心Y轴上的偏移量
    offset_y = float(face_y / img_height - 0.5) * 2

    return (offset_x, offset_y)


 

while True :
    img = getimg()
    # 将彩色图片转换为灰度图
    gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    # 检测画面中的人脸
    faces = FaceCascade.detectMultiScale(
        gray,
        scaleFactor=1.1,
        minNeighbors=5
    )
    # 人脸过滤
    face = face_filter(faces)
    if face is not None:
        # 当前画面有人脸
        (x, y, w, h) = face
        # 在原彩图上绘制矩形
        cv2.rectangle(img, (x, y), (x+w, y+h), (0, 255, 0), 4)
        img_height, img_width,_ = img.shape
        print("img h:{} w:{}".format(img_height, img_width))
        # 计算x轴与y轴的偏移量
        (offset_x, offset_y) = calculate_offset(img_width, img_height, face)
        # 计算下一步舵机要转的角度
        next_btm_degree = btm_servo_control(offset_x)
        next_top_degree = top_servo_control(offset_y)
    
        # 舵机转动
        setDirection(next_btm_degree)
        
            #通过用户输入的角度来改变舵机的角度
        time.sleep(0.1)
        last_btm_degree = next_btm_degree
        last_top_degree = next_top_degree
        print("X轴偏移量：{} Y轴偏移量：{}".format(offset_x, offset_y))
        print('底部角度： {} 顶部角度：{}'.format(next_btm_degree, next_top_degree))
    input=cv2.waitKey(1)
    if input==ord('q'):#如过输入的是q就break，结束图像显示，鼠标点击视频画面输入字符
        break
    cv2.imshow('FaceDetect', img)
    
GPIO.cleanup()
cv2.destroyAllWindows()
cap.release()

